#include <stdint.h>
#include "oslib.h"
#include "comm.h"
#include "chassis.h"
#include "light.h"
#include "ultrasonic.h"
#include "switch.h"
#include "action.h"

extern osMessageQueueId_t dispatchQueueHandle;

int dispatch_run_flag = 1;

void dispatch_start()
{
    uprintf("Start!\n");
    dispatch_run_flag = 1;
}
void dispatch_stop()
{
    uprintf("Stop!\n");
    STOP();
    dispatch_run_flag = 0;
}

void DispatchTask(void *argument) 
{
    static wobot_down_message_payload dmpl;
    static wobot_up_message_ultrasonic ults = {
        .pre = WOBOT_PRE_LE, 
        .idx = WOBOT_IDX, 
        .device_id = SENS_SONIC_VAL,
        .crlf = WOBOT_CRLF_LE,
    };
    static wobot_up_message_button btn = {
        .pre = WOBOT_PRE_LE,
        .idx = WOBOT_IDX,
        .device_id = SENS_BUTTON,
        .crlf = WOBOT_CRLF_LE,
    };
    static wobot_up_message_switch sw = {
        .pre = WOBOT_PRE_LE,
        .idx = WOBOT_IDX,
        .device_id = SENS_SWITCH,
        .crlf = WOBOT_CRLF_LE,
    };
    for (;;) 
    {
        osMessageQueueGet(dispatchQueueHandle, &dmpl, NULL, osWaitForever);
        
        if (!dispatch_run_flag)
            continue;
        
        switch (dmpl.device_id) 
        {
        case ACT_ENCODE_MOTOR:  // 单个编码电机
            uprintf("Motor: id=%hhd, speed=%hhd\n", dmpl.param[1], dmpl.param[2]);
            // param[0]:固定为0x02 param[1]:电机id param[2]:电机速度
            set_speed_25ga(dmpl.param[1], dmpl.param[2]);
            break;
        case ACT_CHASSIS:        // 轨迹
            if (param[0] == 0x02) {
                uprintf("Chassis: radius=%hhd, speed=%hhd\n", dmpl.param[1], dmpl.param[2]);
                // param[0]:0x02 param[1]:底盘半径 param[2]:底盘速度
                Turn_FL(dmpl.param[1], dmpl.param[2]);
            } else if (param[0] == 0x01) {
                uprintf("Chassis: speed=%hhd\n", dmpl.param[1]);
                // param[0]:0x01 param[1]:底盘速度
                Move_LF(dmpl.param[1]);
            }
            break;
        case SENS_SONIC_VAL:    // 超声波
            uprintf("Ultrasonic: id=%hhd\n", dmpl.param[0]);
            ults.port = dmpl.param[0];
            ults.value = ultra_read(dmpl.param[0]);
            if (ults.value <= 0) {
                // 不正确的值, 通过一个非法的port值告知上位机本次读取的值不正确
                ults.port = 255;
            }
            OSLIB_Uart_SendData(&huart3, (uint8_t*)&ults, sizeof(wobot_up_message_ultrasonic));
            uprintf("Ultrasonic: value=%.2f\n", ults.value);
            break;
        case DIS_LIGHT:         // 灯条
            uprintf("Light: id=%hhd, which=%hhd, R=%hhx, G=%hhx, B=%hhx\n", 
                    dmpl.param[0], dmpl.param[1], dmpl.param[2], dmpl.param[3], dmpl.param[4]);
            // param[0]:不使用 param[1]:灯号 param[2]:红色分量 param[3]:绿色分量 param[4]:蓝色分量
            light_set_color(dmpl.param[1], dmpl.param[2], dmpl.param[3], dmpl.param[4]);
            break;
        case ACT_DOOR:
            uprintf("Door: mode=%hhd\n", dmpl.param[1]);
            // param[0]:不使用 param[1]:操作, 0为关, 1为开
            action_door(dmpl.param[1]);
            break;
        case ACT_ARM:
            uprintf("Arm: mode=%hhd\n", dmpl.param[1]);
            // param[0]:不使用 param[1]:操作, 0为关, 1为开
            action_arm(dmpl.param[1]);
            break;
        case ACT_PLATFORM:
            uprintf("Platform: mode=%hhd\n", dmpl.param[1]);
            // param[0]:不使用 param[1]:操作, 0为拉回来, 1为推出去
            action_platform(dmpl.param[1]);
            break;
        case ACT_SIDECAM:
            uprintf("SideCam: dir=%hhd\n", dmpl.param[1]);
            // param[0]:不使用 param[1]:方向, 0为右, 1为左
            action_sidecam(dmpl.param[1]);
            break;
        case ACT_FLAG:
            uprintf("Flag: which=%hhd\n", dmpl.param[1]);
            // param[0]:不使用 param[1]:旗子编号, 1~3
            action_flag(dmpl.param[1]);
            break;
        case SENS_BUTTON:
            uprintf("Button: id=%hhd\n", dmpl.param[0]);          // param[0]:不使用 param[1]:灯号 param[2]:红色分量 param[3]:绿色分量 param[4]:蓝色分量
            btn.port = dmpl.param[0];
            if (HAL_GPIO_ReadPin(BTN_GPIO_Port, BTN_Pin) == GPIO_PIN_SET) {
                btn.value = 0x01;
            }
            else {
                btn.value = 0x00;
            }
            OSLIB_Uart_SendData(&huart3, (uint8_t *)&btn, sizeof(wobot_up_message_button));
            break;
        case SENS_SWITCH:
            uprintf("Switch: id=%hhd, which=%hhd\n", dmpl.param[0], dmpl.param[1]);
            sw.port = dmpl.param[0];
            sw.value = switch_read(dmpl.param[1]);
            OSLIB_Uart_SendData(&huart3, (uint8_t *)&sw, sizeof(wobot_up_message_switch));
            break;
        default:
            uprintf("Unknown device id\n");
            break;
        }
    }
}